/*
 * @Description: 前端里程计算法
 * @Author: Sang Hao
 * @Date: 2021-09-15 17:05:00
 * @LastEditTime: 2021-10-27 14:06:12
 * @LastEditors: Sang Hao
 */

#include <fstream>
#include <boost/filesystem.hpp>
#include <pcl/common/transforms.h>
#include <pcl/io/pcd_io.h>
#include "glog/logging.h"

#include "lidar_slam/global_defination/global_defination.h"
#include "lidar_slam/mapping/front_end/front_end.hpp"
#include "lidar_slam/tools/print_info.hpp"
#include "lidar_slam/models/registration/ndt_registration.hpp"
#include "lidar_slam/models/cloud_filter/voxel_filter.hpp"
#include "lidar_slam/models/cloud_filter/no_filter.hpp"

namespace lidar_slam {

FrontEnd::FrontEnd() 
	:local_map_ptr_(new CloudData::CLOUD()) {
	InitWithConfig();
}

bool FrontEnd::SetInitPose(const Eigen::Matrix4f& init_pose) {
	init_pose_ = init_pose;
	return true;
}


/**
 * @description: 总的初始化函数，导入配置文件，并做初始化，内部调用其他几个初始化函数
 * @param  {*}
 * @return {*}
 */
bool FrontEnd::InitWithConfig() {
	std::string config_file_path = WORK_SPACE_PATH + "/config/front_end.yaml";
	YAML::Node config_node = YAML::LoadFile(config_file_path);

	std::cout << "------------前端模块初始化---------------" << std::endl;
	InitParam(config_node);
	InitRegistration(registration_ptr_, config_node);
	InitFilter("local_map", local_map_filter_ptr_, config_node);
	InitFilter("frame", frame_filter_ptr_, config_node);

	return true;		
}

/**
 * @description: 初始化一些前端里程计需要设置的参数
 * @param  {*}
 * @return {*}
 */
bool FrontEnd::InitParam(const YAML::Node& config_node) {
	key_frame_distance_ = config_node["key_frame_distance"].as<float>();
	local_frame_num_ = config_node["local_frame_num"].as<int>();

	return true;
}

bool FrontEnd::InitFilter(std::string filter_user, 
	std::shared_ptr<CloudFilterInterface>& filter_ptr, const YAML::Node& config_node) {
	std::string filter_method = config_node[filter_user + "_filter"].as<std::string>();
	std::cout << "前端" << filter_user << "选择的滤波方法为：" << filter_method << std::endl;

	if (filter_method == "voxel_filter") {
		filter_ptr = std::make_shared<VoxelFilter>(config_node[filter_method][filter_user]);
	} else if (filter_method == "no_filter") {
		filter_ptr = std::make_shared<NoFilter>();
	} else {
		LOG(ERROR) << "没有为 " << filter_user << " 找到与 " << filter_method << " 相匹配的滤波方法!";
		return false;
	}
	return true;
}


/**
 * @description: 初始化点云匹配方式及其对应的匹配参数
 * @param  {*}
 * @return {*}
 */
bool FrontEnd::InitRegistration(std::shared_ptr<RegistrationInterface>& registration_ptr, 
								const YAML::Node& config_node) {
	std::string registration_method = config_node["registration_method"].as<std::string>();
	std::cout << "前端选择的点云匹配方式为：" << registration_method << std::endl;

	if (registration_method == "NDT") {
		registration_ptr = std::make_shared<NDTRegistration>(config_node[registration_method]);
	} else {
		LOG(ERROR) << "没有找到与" << registration_method << "相对应的匹配方式" << std::endl;
		return false;
	}
	return true;
}

/**
 * @description: 前端总的更新函数
 * @param  {*}
 * @return {*}
 * @param {CloudData} cloud_data
 * @param {Matrix4f} &cloud_pose
 */
bool FrontEnd::Update(const CloudData& cloud_data, Eigen::Matrix4f &cloud_pose) {
	current_frame_.cloud_data.time = cloud_data.time;
	std::vector<int> indices;
	pcl::removeNaNFromPointCloud(
		*cloud_data.cloud_ptr, *current_frame_.cloud_data.cloud_ptr, indices);

	/* 对当前帧的一个滤波 */
	CloudData::CLOUD_PTR filtered_cloud_ptr(new CloudData::CLOUD());
	frame_filter_ptr_->Filter(current_frame_.cloud_data.cloud_ptr, filtered_cloud_ptr);

	static Eigen::Matrix4f step_pose = Eigen::Matrix4f::Identity();
	static Eigen::Matrix4f last_pose = Eigen::Matrix4f::Identity();
	static Eigen::Matrix4f predict_pose = Eigen::Matrix4f::Identity();
	static Eigen::Matrix4f last_key_frame_pose = Eigen::Matrix4f::Identity();

	// 局部地图关键帧容器中没有关键帧，说明是第一帧数据
	// 把当前帧当做第一个关键帧，插入到局部地图中去，即调用UpdateWithNewFrame
	if (local_map_frames_.size() == 0) {
		current_frame_.pose = init_pose_;
		UpdateWithNewFrame(current_frame_);
		cloud_pose = current_frame_.pose;
		return true;
	}
	
	// 否则，即不是第一帧，则正常匹配即可
	CloudData::CLOUD_PTR result_cloud_ptr(new CloudData::CLOUD());
	/**/
	// 这里暂时略过了滤波部分，因此input_source取的是current_frame_...
	registration_ptr_->ScanMatch(filtered_cloud_ptr, 
		predict_pose, result_cloud_ptr, current_frame_.pose);
	cloud_pose = current_frame_.pose;
	
	// 更新相邻两帧的相对运动
	// 上一帧的姿态的逆乘以当前帧就是当前相对与上一帧的姿态变化step_size
	step_pose = last_pose.inverse() * current_frame_.pose;
	predict_pose = current_frame_.pose * step_pose;
	last_pose = current_frame_.pose;

	// 匹配后看是否需要更新关键帧，曼哈顿距离超过阈值key_fram_distance_就更新新的关键帧
	if (fabs(last_key_frame_pose(0, 3) - current_frame_.pose(0, 3)) +
		fabs(last_key_frame_pose(1, 3) - current_frame_.pose(1, 3)) + 
		fabs(last_key_frame_pose(2, 3) - current_frame_.pose(2, 3)) > key_frame_distance_) {
		
		UpdateWithNewFrame(current_frame_);
		last_key_frame_pose = current_frame_.pose;
	}
	return true;
	
}

/**
 * @description: 将关键帧new_key_frame加入到关键帧队列local_map_frame_中，并更新局部地图local_map_ptr_ 
 * @param  {*}
 * @return {*}
 */
bool FrontEnd::UpdateWithNewFrame(const Frame& new_key_frame) {	
	// 这一步是为了把关键帧点云保存下来
	// 因为用的是共享指针，所以直接赋值相当于只是复制了一根指针而已
	// 此时无论放多少关键帧在容器里，这些关键帧点云指针指向的是同一个点云
	Frame key_frame = new_key_frame;
	key_frame.cloud_data.cloud_ptr.reset(
		new CloudData::CLOUD(*new_key_frame.cloud_data.cloud_ptr));

	// 新定义一个点云，用来存转换后的点云
	CloudData::CLOUD_PTR transformed_cloud_ptr(new CloudData::CLOUD());

	// 更新局部地图
	local_map_frames_.push_back(key_frame);

	// 弹出超过数目限制的关键帧
	while (local_map_frames_.size() > static_cast<size_t>(local_frame_num_)) {
		local_map_frames_.pop_front();
	}

	// 更新局部地图，把这些局部地图所包含的关键帧统统转换到世界系点云，然后加到一块儿；
	local_map_ptr_.reset(new CloudData::CLOUD());
	for (size_t i = 0; i < local_map_frames_.size(); ++i) {
		pcl::transformPointCloud(*local_map_frames_.at(i).cloud_data.cloud_ptr,
				*transformed_cloud_ptr, local_map_frames_.at(i).pose);
		
		*local_map_ptr_ += *transformed_cloud_ptr;
	}

	// 加入voxel_filter，更新局部地图点云，这里先暂时不加滤波
	// 更新ndt匹配的目标点云 ： registration_ptr_->SetInputTarget(......)
	// 关键帧数量少的时候不滤波，因为点云太少了
	// 其实这个10也可以加入到config_node中，然后类中加个private变量
	if (local_map_frames_.size() < 10) {
		registration_ptr_->SetInputTarget(local_map_ptr_);
	} else {
		CloudData::CLOUD_PTR filtered_local_map_ptr(new CloudData::CLOUD());
		local_map_filter_ptr_->Filter(local_map_ptr_, filtered_local_map_ptr);
		registration_ptr_->SetInputTarget(filtered_local_map_ptr);
	}

	return true;
}
}